# -*- encoding:UTF-8 -*-
from naoqi import ALProxy
import time
import threading
import cmath


IP = "192.168.1.103"
PORT = 9559
global motionProxy
global ttsProxy
global postureProxy


def standUp():
    postureProxy.goToPosture("Stand", 0.5)
    pass


def stretchArms():
    # ttsProxy.say("伸展手臂")
    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LShoulderRoll", 70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", 73.4 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LShoulderPitch", 73.4 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LWristYaw", -100 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 4.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LElbowRoll", -4.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RHand", 1.0, 0.2)
    # motionProxy.setAngles("LHand", 1.0, 0.2)
    time.sleep(1)
    pass


def curlArms():
    # ttsProxy.say("弯曲手臂")
    motionProxy.setAngles("RShoulderPitch", 0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LShoulderPitch", 0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LShoulderRoll", 70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LElbowRoll", -2.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LWristYaw", -100 * cmath.pi / 180, 0.2)
    # time.sleep(1)

    # motionProxy.setAngles("RElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    pass


def glueTheBoll():
    motionProxy.setAngles("RShoulderRoll", 0 * cmath.pi / 180, 0.1)
    time.sleep(1)

    motionProxy.setAngles("RElbowYaw", -100 * cmath.pi / 180, 0.1)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", -45 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderRoll", 18 * cmath.pi / 180, 0.1)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", 0 * cmath.pi / 180, 0.1)
    time.sleep(2)

    motionProxy.setAngles("RElbowRoll", 12.8 * cmath.pi / 180, 0.1)
    time.sleep(3)
    # ttsProxy.say("成功")
    pass


def backCurlArms():
    motionProxy.setAngles("RElbowRoll", 0 * cmath.pi / 180, 0.05)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", -45 * cmath.pi / 180, 0.05)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", -100.0 * cmath.pi / 180, 0.05)
    time.sleep(3)

    # motionProxy.setAngles("RElbowYaw", 68 * cmath.pi / 180, 0.1)
    # time.sleep(1)

    # motionProxy.setAngles("RElbowRoll", 15 * cmath.pi / 180, 0.1)
    # time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", -76 * cmath.pi / 180, 0.05)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", 82.7 * cmath.pi / 180, 0.05)
    time.sleep(3)

    motionProxy.setAngles("RShoulderRoll", -25.0 * cmath.pi / 180, 0.05)
    time.sleep(1)

    # motionProxy.setAngles("RElbowYaw", 11.0 * cmath.pi / 180, 0.1)
    # time.sleep(1)


    motionProxy.setAngles("RShoulderPitch", 42 * cmath.pi / 180, 0.05)
    time.sleep(3)

    motionProxy.setAngles("RElbowYaw", 26.5 * cmath.pi / 180, 0.05)
    time.sleep(3)

    motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.05)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 80 * cmath.pi / 180, 0.05)
    time.sleep(1)
    motionProxy.setAngles("LShoulderPitch", 115 * cmath.pi / 180, 0.03)
    time.sleep(5)
    pass


def move():
    motionProxy.setMoveArmsEnabled(False, False)
    motionProxy.wakeUp()
    # motionProxy.move(0.1, 0.0, 90, moveConfig)
    motionProxy.moveTo(0, 0, cmath.pi / 2)
    pass


def back():
    for i in range(10):
        motionProxy.move(-0.03, 0, 0, moveConfig)
        time.sleep(1)
    motionProxy.stopMove()
    pass


class turnAround(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)

    def run(self):
        motionProxy.setMoveArmsEnabled(False, False)
        initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
        while True:
            motionProxy.move(0, 0, 0.1, moveConfig)
            nowtangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
            dis = (nowtangle - initangle) * 180 / cmath.pi
            print dis
            while dis >= 90:
                motionProxy.stopMove()
                break
            time.sleep(1)


def washFruit():
    motionProxy.setAngles("RShoulderPitch", -10 * cmath.pi / 180, 0.05)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 3 * cmath.pi / 180, 0.05)
    time.sleep(1)

    for i in range(3):
        motionProxy.setAngles("RShoulderRoll", 17 * cmath.pi / 180, 0.02)
        time.sleep(3)

        motionProxy.setAngles("RShoulderRoll", -15 * cmath.pi / 180, 0.02)
        time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 80 * cmath.pi / 180, 0.05)
    time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", -25.0 * cmath.pi / 180, 0.05)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", 42 * cmath.pi / 180, 0.05)
    time.sleep(1)
    pass


if __name__ == '__main__':
    # 单手步态
    # maxstepx = 0.05
    # maxstepy = 0.08
    # maxsteptheta = 0.2
    # maxstepfrequency = 0.2
    # maxstepheight = 0.01
    # torsowx = 0.0
    # torsowy = 0.0

    # 后退步态
    maxstepx = 0.02
    maxstepy = 0.05
    maxsteptheta = 0.2
    maxstepfrequency = 0.2
    maxstepheight = 0.01
    torsowx = 0.0
    torsowy = 0.0

    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepheight],
                  ["MaxStepFrequency", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]

    motionProxy = ALProxy("ALMotion", IP, PORT)
    ttsProxy = ALProxy("ALTextToSpeech", IP, PORT)
    postureProxy = ALProxy("ALRobotPosture", IP, PORT)
    memorypxy = ALProxy("ALMemory", IP, PORT)
    motionProxy.wakeUp()
    # motionProxy.moveTo(0.1, 0.0, 0.0)
    # time.sleep(2)

    # 站直
    standUp()

    # 伸展手臂
    stretchArms()

    # 弯曲手臂
    curlArms()

    # 黏住小球
    glueTheBoll()

    # 收回手臂
    backCurlArms()

    # move()

    # 后退
    back()

    # 转身
    turn = turnAround()
    turn.start()


    # 走到水槽

    # 洗水果
    washFruit()